#!/usr/bin/env python

#
# This is the non graphical realtime simulator
#
import gobject
from optparse import OptionParser
from datetime import datetime

import numpy as np

import pat.frames                       as pfr
import pat.dynamic_model                as pdm
import pat.autopilot                    as pap
import pat.input_output.hudson_kahn_sim as phk
import pat.input_output.joystick        as pjs
import pat.input_output.flightgear      as pfg

class TerminalOutput:
    def __init__(self):
        np.set_printoptions(precision=2, suppress=False, linewidth=160)
    
    def _print(self, X, U):
        print("{:s}{:s}".format(str(X), str(U)))


class Simulator(gobject.GObject):

    __gsignals__ = { 
        "updated" : (gobject.SIGNAL_RUN_LAST, gobject.TYPE_NONE,())
        } 

    def __init__(self, options, term_out=False):
        gobject.GObject.__init__(self)
#        self.dt_usec = 1000/50
        self.dt_usec = 1000/100 
        self.timer = None
        self.time = 0.

        if (options.verbose or term_out): self.term_out = TerminalOutput()
        else: self.term_out = None

        if options.hksrv: self.hk_out = phk.Server()
        else: self.hk_out = None

        if options.fg_host: self.fg = pfg.FlightGear(options.fg_host, 5501, self)
        else: self.fg = None

        if options.heli : dm_type, dm_variant = "rotorcraft", "hk"
        elif options.planar_vtol: dm_type, dm_variant = "rotorcraft", "planar_simple"
        else: dm_type, dm_variant = options.dyn_cfg.split(":")
        print "#dyn: ", dm_type, dm_variant
        self.dm = pdm.load(type=dm_type, variant=dm_variant, param=options.dyn_filename)

        if options.heli : ctl_type, ctl_variant = "rotorcraft", "hk"
        elif options.planar_vtol: ctl_type, ctl_variant = "rotorcraft", "planar"
        else: ctl_type, ctl_variant = options.ctl_cfg.split(":")
        print "#ctl: ", ctl_type, ctl_variant
        self.ap = pap.load(type=ctl_type, variant=ctl_variant, _dm=self.dm, _args=options.ap_filename)

        # not sure where to put that
        self.X6EE = np.zeros(pfr.SixDOFfEuclidianEuler.size)
        
 
    def toggle_on_off(self):
        if self.timer<>None:
            gobject.source_remove(self.timer)
            self.timer = None
        else:
            self.timer = gobject.timeout_add(self.dt_usec, self.periodic_callback)
            self.last_periodic = datetime.now()

    def is_running(self): return (self.timer<>None)

    def reset(self):
        self.time = 0.
        if self.is_running(): self.last_periodic = datetime.now()
        self.dm.reset();
        self.emit("updated")
    
    def periodic_callback(self):
        now = datetime.now(); dt = now-self.last_periodic; self.last_periodic = now
        dt_sec = dt.seconds+dt.microseconds*1e-6
        dt_sec = float(self.dt_usec)*1e-3
        self.time += dt_sec
        self.ap.run(self.time, self.dm.X)
        self.dm.run(dt_sec, self.ap.Ui)
        self.X6EE = self.dm.state_SixDOFfEuclidianEuler()
        self.emit("updated")
        if self.term_out<>None: self.term_out._print(self.dm.X, self.ap.U)
        if self.hk_out<>None: self.hk_out.send(self.X6EE)
        return True

    def get_time(self):
        return self.time


def parse_cmd_line(parser=None):
    if parser == None:
        usage = "usage: %prog [options]"
        version = "%prog 1.0"
        parser = OptionParser(usage=usage, version=version)

    # configuration of dynamics
    parser.add_option("-d", "--dyn", dest="dyn_cfg",
                      action="store", default="fixed_wing:python_basic",
                      help="dynamic model type")
    parser.add_option("-f", "--file", dest="dyn_filename",
                      action="store", default="../config/Rcam_single_engine.xml",
                      help="dynamic model parameters filename")
    # configuration of control
    parser.add_option("-c", "--ctl", dest="ctl_cfg",
                      action="store", default="fixed_wing:default",
                      help="ctl type")
    parser.add_option("-a", "--ap", dest="ap_filename",
                      action="store", default="../config/ap_default.xml",
                      help="dynamic model parameters filename")

    # shortcut
    parser.add_option("", "--heli", dest="heli",
                      action="store_true",
                      help="default rotorcratf configuration")
    parser.add_option("", "--planar_vtol", dest="planar_vtol",
                      action="store_true",
                      help="default planar rotorcratf configuration")

    # input/ouptut
    parser.add_option("-j", "--joystick", dest="js_device",
                      action="store_true",
                      help="joystick device")
    parser.add_option("-g", "--fghost", dest="fg_host",
                      action="store",
                      help="flightgear host")
    parser.add_option("-k", "--hksrv", dest="hksrv",
                      action="store_true", help="output to Kahn/Hudson 3D view")
    parser.add_option("-v", "--verbose", dest="verbose",
                      action="store_true", help="verbose")
    (options, args) = parser.parse_args()
    return options


if __name__ == "__main__":
    options = parse_cmd_line()
    sim = Simulator(options)
    sim.toggle_on_off()
    mainloop = gobject.MainLoop()
    mainloop.run()
